---@diagnostic disable: undefined-global
import("System")
import("System.IO.Ports")
import("Jodell.Tool", "Jodell.Tool")
--打开串口
COMID = "COM3"
slave_ID = 4
Sensor_ID = 99
--modbus寄存器地址
RestartReg = 1000     -- 重启
EnableReg = 1005      -- 使能
ResetReg = 1006       -- 清错
ActCmdReg = 1007      -- 动作
P0POSReg = 1009       -- 点0位置
P0SPDReg = 1010       -- 点0速度
P0TQEReg = 1011       -- 点0电流/力矩
P1POSReg = 1012       -- 点1位置
P1SPDReg = 1013       -- 点1速度
P1TQEReg = 1014       -- 点1电流/力矩
TorqueRefReg = 114    -- 力矩参考
TorqueFbkReg = 126    -- 力矩反馈
DevSaveReg = 300      -- 设备参数保存
TqeCalibModeReg = 311 -- 力控标定模式
Sensor_Reg = 0        -- 传感器数据
SlowPosReg = 1026     -- 减速段位置百分比
SLowSpdReg = 1027     -- 减速段速度百分比
SlowDirReg = 1028     -- 减速段作用方向

--参数设置
P0POS = 0                 -- 位置0位置
P0SPD = 20000             -- 位置0速度
P0Cur = 100               -- 位置0电流
P0TQE = 100               -- 位置0力矩
P1POS = 2000              -- 位置1位置
P1SPD = 20000             -- 位置1速度
I_min = 200               --标定电流最小值mA
I_max = 1400              --标定电流最大值mA
I_accuracy = 10           --标定电流精度mA
F_min = 30                --验证力最小值N
F_max = 100               --验证力最大值N
F_accuracy = 1            --验证力精度
TimeForPushWaiting = 1500 --推等待时间ms
TimeForPullWaiting = 1000 --拉等待时间ms
SLowPos = 20              --减速段位置百分比
SlowSpd = 5               --减速段速度百分比
SlowDir = 1               --减速段作用方向
client = Lua_ModbusRtuClient()
client.PortName = COMID
client:Connect()

-- 创建 CSV 对象并打开

ret, SN = client:ReadString(slave_ID, 562, 16)
index = string.find(SN, "\0")
SN = string.sub(SN, 1, index - 1)
Info(string.format("设备编号%s", SN))
localpath = string.format("./ForceCtrl\\推力测试(%s).csv", SN)
Info(localpath)
writer = Lua_CsvWriter(localpath)

writer:Open()

writer:WriteString("参考电流(A)")
writer:WriteSplitter()
writer:WriteString("反馈电流(A)")
writer:WriteSplitter()
writer:WriteString("测量压力(N)")
writer:WriteSplitter()
--writer:WriteString("设备反馈压力")
--writer:WriteSplitter()
writer:WriteString("设置电流(mA)")
writer:WriteSplitter()
writer:WriteLine()

--设备使能
ret = client:WriteUInt16(slave_ID, EnableReg, 1)
if ret ~= 0 then
	Error("设置设备使能失败")
	goto exit
end
--设备复位
ret = client:WriteUInt16(slave_ID, ResetReg, 1)
if ret ~= 0 then
	Error("设置设备复位失败")
	goto exit
end
--切换到标定模式
ret = client:WriteUInt16(slave_ID, TqeCalibModeReg, 1)
if ret ~= 0 then
	Error("设置标定模式失败")
	goto exit
end
Delay(100)

-- 减速段配方设置
ret = client:WriteUInt16(slave_ID, SlowPosReg, SLowPos)
if ret ~= 0 then
	Error("设置减速位置失败")
	goto exit
end

ret = client:WriteUInt16(slave_ID, SLowSpdReg, SlowSpd)
if ret ~= 0 then
	Error("设置减速速度失败")
	goto exit
end

ret = client:WriteUInt16(slave_ID, SlowDirReg, SlowDir)
if ret ~= 0 then
	Error("设置减速方向失败")
	goto exit
end

ret = client:WriteUInt16(slave_ID, P0POSReg, P0POS)
if ret == 0 then
	Info(string.format("设置位置0位置:%dmm", P0POS / 100))
else
	Error(string.format("设置位置0位置:%dmm失败", P0POS / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P0SPDReg, P0SPD)
if ret == 0 then
	Info(string.format("设置位置0速度:%dmm/s", P0SPD / 100))
else
	Error(string.format("设置位置0速度:%dmm/s失败", P0SPD / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P0TQEReg, P0Cur)
if ret == 0 then
	Info(string.format("设置位置0电流:%dmA", P0Cur))
else
	Error(string.format("设置位置0电流:%dmA失败", P0Cur))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P1POSReg, P1POS)
if ret == 0 then
	Info(string.format("设置位置1位置:%dmm", P1POS / 100))
else
	Error(string.format("设置位置1位置:%dmm失败", P1POS / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P1SPDReg, P1SPD)
if ret == 0 then
	Info(string.format("设置位置1速度:%dmm/s", P1SPD / 100))
else
	Error(string.format("设置位置1速度:%dmm/s失败", P1SPD / 100))
	goto exit
end
--开始测试
for torque = I_min, I_max, I_accuracy do
	--终止测试
	if (IsCancel) then
		Warn("手动终止")
		goto exit
		break
	end
	--读取运行状态
	--ret,runstatus = client:ReadUInt16(1,2000)
	--if ret ~= 0 or runstatus ~= 4 then
	--Error("切换运行模式失败")
	--goto exit
	--end
	--设置位置1电流
	ret = client:WriteUInt16(slave_ID, P1TQEReg, torque)
	if ret == 0 then
		Info(string.format("设置位置1电流:%dmA", torque))
	else
		Error(string.format("设置位置1电流:%dmA失败", torque))
		goto exit
	end
	--设置速度

	--设置执行动作1
	ret = client:WriteUInt16(slave_ID, ActCmdReg, 1)
	if ret ~= 0 then
		Error("执行动作1失败")
		goto exit
	end
	Delay(TimeForPushWaiting)
	--读电流参考
	ret, torque_ref = client:Read03FloatCDAB(slave_ID, TorqueRefReg)
	if ret ~= 0 then
		Error("读电流参考失败")
		goto exit
	end
	Info(string.format("位置1电流参考:%fA", torque_ref))
	--读电流反馈
	ret, torque_fbk = client:Read03FloatCDAB(slave_ID, TorqueFbkReg)
	if ret ~= 0 then
		Error("读电流反馈失败")
		goto exit
	end
	Info(string.format("推电流反馈:%fA", torque_fbk))
	--读压力
	ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
	if ret ~= 0 then
		ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
		if ret ~= 0 then
			ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
			if ret ~= 0 then
				Error("读压力测量失败")
				goto exit
			end
		end
	end
	torque_mes = torque_mes * 9.8
	Info(string.format("测量压力:%fN", torque_mes))
	--ret,force_fbk = client:ReadUInt16(1,2004)
	--if ret ~=0 then
	--	Error("读设备反馈压力失败")
	--	goto exit
	--end
	--Info(string.format("设备反馈压力:%dN",force_fbk))
	--保存数据
	writer:WriteFloat(-torque_ref)
	writer:WriteSplitter()
	writer:WriteFloat(-torque_fbk)
	writer:WriteSplitter()
	writer:WriteFloat(torque_mes)
	writer:WriteSplitter()
	--writer:WriteFloat(force_fbk)
	--writer:WriteSplitter()
	writer:WriteFloat(torque)
	writer:WriteSplitter()
	writer:WriteLine()
	Delay(100)
	--设置拉动作
	ret = client:WriteUInt16(slave_ID, ActCmdReg, 0)
	if ret ~= 0 then
		Error("执行动作0失败")
		goto exit
	end
	Delay(TimeForPullWaiting)
end

--关闭CSV文件
writer:Close()
client:Disconnect()

SalveNo = slave_ID                               --Modbus从站号
PortName = COMID                                 --串口名称
fileName = localpath                             --CSV文件路径
xIndex = 0                                       --电流 在文件中第几列，下标从0开始
yIndex = 2                                       --实际力 在文件中第几列，下标从0开始

pso_client = Lua_PsoClient()                     --线性拟合操作对象
client = Lua_ModbusRtuClient()                   --Modbus通讯对象
pso_client:LoadCsvData(fileName, xIndex, yIndex) --加载数据
pso_client:Optimize(1000)                        --迭代计算分割点
pso_client:FitData()                             --线性拟合
if IsCancel then
	return
end
--打印拟合信息
Info(string.format("共 %d 个分割点", pso_client.LineSegments.Count))

for i = 0, pso_client.LineSegments.Count - 1 do
	Info(string.format("K:%f \tB:%f \tMinValue:%f \tMaxValue:%f",
		pso_client.LineSegments[i].K,
		pso_client.LineSegments[i].B,
		pso_client.LineSegments[i].MinValue,
		pso_client.LineSegments[i].MaxValue))
	if IsCancel then
		return
	end
end
--Modbus从站连接
client.PortName = PortName
client:Connect()

if IsCancel then
	return
end

client:WriteUInt16(SalveNo, 411, pso_client.LineSegments.Count)                   --下发分段数
client:WriteFloatCDAB(SalveNo, 438, pso_client.LineSegments[0].MinValue)          --下发上边界
for i = 0, pso_client.LineSegments.Count - 1 do
	client:WriteFloatCDAB(SalveNo, 440 + i * 2, pso_client.LineSegments[i].MaxValue) --下发分割点
	client:WriteFloatCDAB(SalveNo, 450 + i * 2, pso_client.LineSegments[i].K)
	client:WriteFloatCDAB(SalveNo, 460 + i * 2, pso_client.LineSegments[i].B)

	if IsCancel then
		return
	end
end
client:WriteUInt16(SalveNo, DevSaveReg, 1) --保存参数
client:Disconnect()
Info("校准完成");
pso_client:SaveImage("./data.png")

--力控验证
Info("开始验证")
--
writer = Lua_CsvWriter(string.format("./ForceCtrl/验证结果(%s).csv", SN))
writer:Open()

writer:WriteString("参考电流(A)")
writer:WriteSplitter()
writer:WriteString("反馈电流(A)")
writer:WriteSplitter()
writer:WriteString("测量压力(N)")
writer:WriteSplitter()
--writer:WriteString("设备反馈压力")
--writer:WriteSplitter()
writer:WriteString("设置压力(N)")
writer:WriteSplitter()
writer:WriteLine()

client = Lua_ModbusRtuClient()
client.PortName = COMID
client:Connect()

--设备重启
ret = client:WriteUInt16(slave_ID, RestartReg, 1)
Delay(2000)

--设备使能
ret = client:WriteUInt16(slave_ID, EnableReg, 1)
if ret ~= 0 then
	Error("设置设备使能失败")
	goto exit
end
--设备复位
ret = client:WriteUInt16(slave_ID, ResetReg, 1)
if ret ~= 0 then
	Error("设置设备复位失败")
	goto exit
end

-- 减速段配方设置
ret = client:WriteUInt16(slave_ID, SlowPosReg, SLowPos)
if ret ~= 0 then
	Error("设置减速位置失败")
	goto exit
end

-- 动作配方设置
ret = client:WriteUInt16(slave_ID, SLowSpdReg, SlowSpd)
if ret ~= 0 then
	Error("设置减速速度失败")
	goto exit
end

ret = client:WriteUInt16(slave_ID, SlowDirReg, SlowDir)
if ret ~= 0 then
	Error("设置减速方向失败")
	goto exit
end

--切换到力控模式
ret = client:WriteUInt16(slave_ID, TqeCalibModeReg, 0)
if ret ~= 0 then
	Error("设置力控模式失败")
	goto exit
end
Delay(100)

ret = client:WriteUInt16(slave_ID, P0POSReg, P0POS)
if ret == 0 then
	Info(string.format("设置位置0位置:%dmm", P0POS / 100))
else
	Error(string.format("设置位置0位置:%dmm失败", P0POS / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P0SPDReg, P0SPD)
if ret == 0 then
	Info(string.format("设置位置0速度:%dmm/s", P0SPD / 100))
else
	Error(string.format("设置位置0速度:%dmm/s失败", P0SPD / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P0TQEReg, P0TQE)
if ret == 0 then
	Info(string.format("设置位置0力矩:%dN", 100))
else
	Error(string.format("设置拉力:%dN失败", 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P1POSReg, P1POS)
if ret == 0 then
	Info(string.format("设置位置1位置:%dmm", P1POS / 100))
else
	Error(string.format("设置位置1位置:%dmm失败", P1POS / 100))
	goto exit
end

ret = client:WriteUInt16(slave_ID, P1SPDReg, P1SPD)
if ret == 0 then
	Info(string.format("设置位置1速度:%dmm/s", P1SPD / 100))
else
	Error(string.format("设置位置1速度:%dmm/s失败", P1SPD / 100))
	goto exit
end
--开始测试
for torque = F_min, F_max, F_accuracy do
	--终止测试
	if (IsCancel) then
		Warn("手动终止")
		goto exit
		break
	end
	--读取运行状态
	--ret,runstatus = client:ReadUInt16(1,2000)
	--if ret ~= 0 or runstatus ~= 4 then
	--Error("切换运行模式失败")
	--goto exit
	--end
	--设置推力
	ret = client:WriteUInt16(slave_ID, P1TQEReg, torque)
	if ret == 0 then
		Info(string.format("设置推力:%dN", torque))
	else
		Error(string.format("设置推力:%dN失败", torque))
		goto exit
	end
	--设置推动作
	ret = client:WriteUInt16(slave_ID, ActCmdReg, 1)
	if ret ~= 0 then
		Error("设置推动作失败")
		goto exit
	end
	Delay(TimeForPushWaiting)
	--读电流参考
	ret, torque_ref = client:Read03FloatCDAB(slave_ID, TorqueRefReg)
	if ret ~= 0 then
		Error("读电流参考失败")
		goto exit
	end
	Info(string.format("推电流参考:%fA", torque_ref))
	--读电流反馈
	ret, torque_fbk = client:Read03FloatCDAB(slave_ID, TorqueFbkReg)
	if ret ~= 0 then
		Error("读电流反馈失败")
		goto exit
	end
	Info(string.format("推电流反馈:%fA", torque_fbk))
	--读压力
	ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
	if ret ~= 0 then
		ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
		if ret ~= 0 then
			ret, torque_mes = client:Read04FloatABCD(Sensor_ID, Sensor_Reg)
			if ret ~= 0 then
				Error("读压力测量失败")
				goto exit
			end
		end
	end
	torque_mes = torque_mes * 9.8
	Info(string.format("测量压力:%fN", torque_mes))

	writer:WriteFloat(-torque_ref)
	writer:WriteSplitter()
	writer:WriteFloat(-torque_fbk)
	writer:WriteSplitter()
	writer:WriteFloat(torque_mes)
	writer:WriteSplitter()
	--writer:WriteFloat(force_fbk)
	--writer:WriteSplitter()
	writer:WriteFloat(torque)
	writer:WriteSplitter()
	writer:WriteLine()
	Delay(100)
	-- 执行动作0
	ret = client:WriteUInt16(slave_ID, ActCmdReg, 0)
	if ret ~= 0 then
		Error("设置拉动作失败")
		goto exit
	end
	Delay(TimeForPullWaiting)
end

--退出
::exit::
--关闭CSV文件
writer:Close()
--关闭串口
client:Disconnect()
Info("测试结束")
Info("打开文件查看结果")
